#include "DM_motor/functionMenu.h"
namespace DM_motor {
    functionMenu::functionMenu() {
        serial = std::make_shared<SerialPort>("/dev/ttyACM0", B921600);
        dm = Motor_Control(serial);
    }
    functionMenu::~functionMenu() {

    }
    DM_Code functionMenu::MotorRun_PV(Motor m,float pos,float vel){
        dm.addMotor(&m);
        dm.disable(m);
        sleep(1);
        if (dm.switchControlMode(m, POS_VEL_MODE))
        {
            std::cout << "Switch to POS_VEL_MODE Success" << std::endl;
            dm.save_motor_param(m);
            dm.enable(m);
            sleep(1);
            float q = sin(std::chrono::system_clock::now().time_since_epoch().count() / 1e9);
            int flag=1;
            dm.control_pos_vel(m, pos, vel);
            while (flag)
            {
                dm.refresh_motor_status(m);
                float localPos=m.Get_Position();
                std::cout<<"Motor local Pos: "<<localPos<<std::endl;
                if(fabs(localPos-pos)<1.0e-2&&fabs(localPos-pos)>-(1.0e-2)){
                    flag=0;
                }
                usleep(50000);//50ms
            }
            dm.disable(m);
        }
        return DM_OK;
    }
}
